import numpy as np
import cv2 as cv
import os
import math
from matplotlib import pyplot as plt
import sys
import numpy as np
import cv2
import math

if not os.path.exists('picout'): os.mkdir('picout')

UNDISTORT = True

x1c=34 
y1c=45 #画面中xy各有多少点，如果算不出来尝试调换xy顺序
fname='pic/full2.jpg' #input image name 需要裁剪掉画面中半个圆形
BinaryThreshold = False # 对图像是否二值化

blobParams = cv2.SimpleBlobDetector_Params()
blobParams.blobColor = 0
blobParams.minThreshold = 0
blobParams.maxThreshold = 255
blobParams.filterByArea = True
blobParams.minArea = 100    
blobParams.maxArea = 300
blobParams.filterByCircularity = False
blobParams.minCircularity = 0.00001
blobParams.filterByConvexity = False
blobParams.minConvexity = 0.1
blobParams.filterByInertia = False
blobParams.minInertiaRatio = 0.01
blobDetector = cv2.SimpleBlobDetector_create(blobParams)

img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
if BinaryThreshold:
    th, gray = cv2.threshold(gray, 50, 255, cv2.THRESH_BINARY)
    cv2.imwrite('picout/'+'bin.bmp',gray)

keypoints = blobDetector.detect(gray) # Detect blobs.
print('blob detector find keypoints count:',len(keypoints))

im_with_keypoints = cv2.drawKeypoints(img, keypoints, np.array([]), (0,255,0), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS) #绿色圈描绘所有找到的blobs
cv2.imwrite('picout/'+'keypoints.bmp',im_with_keypoints)

p = cv2.CirclesGridFinderParameters()
ret, corners = cv2.findCirclesGrid(gray, (x1c,y1c), (cv2.CALIB_CB_SYMMETRIC_GRID+ + cv2.CALIB_CB_CLUSTERING),  blobDetector, p)   # Find the circle grid
if ret:
    print('successfully detected',x1c,y1c)

    # draw images with chessboard circles
    img = cv2.imread(fname)
    cv2.drawChessboardCorners(img, (x1c,y1c), corners, ret)
    cv2.imwrite('picout/'+'fullcirc.jpg', img)

    objp = np.zeros((x1c*y1c,3), np.float32)
    objp[:,:2] = np.mgrid[0:x1c,0:y1c].T.reshape(-1,2) #这里要调换顺序！确保corners的顺序跟objp的顺序相同！

    corners = np.array([ [[i[0][0],1600-i[0][1]],] for i in corners],dtype=np.float32) #这里要调换顺序！确保corners的顺序跟objp的顺序相同！

    dists = []
    for idx in range(len(corners)-1):
        dist = math.sqrt(math.pow(corners[idx][0][0]-corners[idx+1][0][0],2)+math.pow(corners[idx][0][1]-corners[idx+1][0][1],2))
        dists.append(dist)
    avg = np.mean([i for i in dists if i<68])
    



    objpoints = [objp,]
    imgpoints = [corners,]
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape, None, None)#标定

    print(ret, mtx, dist, rvecs, tvecs) #标定结果
    mean_error = 0 #计算重投影误差
    for i in range(len(objpoints)):
        imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
        error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2)/len(imgpoints2)
        mean_error += error
        print( "total reprojection error: {}".format(mean_error/len(objpoints)) )


#get the distance from a point to a line defined with two points
def getdis(point_x,point_y,x1,y1,x2,y2):
    array_longi = np.array([x2 - x1, y2 - y1])
    array_trans = np.array([x2 - point_x, y2 - point_y])
    # 用向量计算点到直线距离
    array_temp = (float(array_trans.dot(array_longi)) / array_longi.dot(array_longi))  # 注意转成浮点数运算
    array_temp = array_longi.dot(array_temp)
    distance = np.sqrt((array_trans - array_temp).dot(array_trans - array_temp))
    return distance


inputfolder="./Pic331/"
for name in os.listdir(inputfolder):
    if "bmp" not in name:continue



    
    img = cv.imread(os.path.join(inputfolder,name),0)
    h,  w = img.shape[:2]
    newcameramtx, roi = cv2 .getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))
    if UNDISTORT: img = cv2.undistort(img, mtx, dist, None, newcameramtx)
    cv2.imwrite('picout/'+name[:-4]+'out1.bmp',img)
   
    
    img[:5,:]=255
    img[:,:5]=255
    img[-5:,:]=255
    img[:,-5:]=255
    
    blobParams = cv.SimpleBlobDetector_Params()
    blobParams.blobColor = 0
    blobParams.minThreshold = 0
    blobParams.maxThreshold = 255
    blobParams.filterByArea = True
    blobParams.minArea = 10000    
    blobParams.maxArea = 1000000
    blobParams.filterByCircularity = False
    blobParams.minCircularity = 0.00001
    blobParams.filterByConvexity = False
    blobParams.minConvexity = 0.1
    blobParams.filterByInertia = False
    blobParams.minInertiaRatio = 0.01
    blobDetector = cv.SimpleBlobDetector_create(blobParams)
    keypoints = blobDetector.detect(img) # Detect blobs.
    img1 = cv.drawKeypoints(img, keypoints, np.array([]), (0,255,0), cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS) #绿色圈描绘所有找到的blobs
    cv2.imwrite('picout/'+name[:-4]+'out2.bmp',img1)
    assert(len(keypoints)==2)
    img = cv.Canny(img,60,90)
    cv2.imwrite('picout/'+name[:-4]+'out3.bmp',img)
    


    Houghline_threshold=100
    linesP=cv.HoughLines(img,1,math.pi/180,Houghline_threshold,None,0,10000)
    img=cv.cvtColor(img,cv.COLOR_GRAY2RGB)
    linesnum=0
    line_point=[]
    line_ab=[]
    for line in linesP:
            linesnum+=1
            for rho, theta in line:
                if rho == 5 or theta == 0: continue
                a = np.cos(theta)
                b = np.sin(theta)
                x0 = a * rho
                y0 = b * rho
                x1 = int(x0 + 500 * (-b))
                y1 = int(y0 + 500 * (a))
                x2 = int(x0 - 500 * (-b))
                y2 = int(y0 - 500 * (a))
                if abs(-1/np.tan(theta))<0.001: continue
                if abs(np.tan(theta))<0.001: continue
                line_point.append([x1,y1,x2,y2])
                line_ab.append([-1/np.tan(theta), x0,y0,theta])

    #line_point.sort(key=lambda x:math.sqrt(math.pow(x[2]-x[0],2)+math.pow(x[3]-x[1],2)), reverse=True)
    '''
    md =[10000,10000]
    bi = [0,0]
    for idx,k in enumerate(keypoints):
        for i in range(len(line_point)):
            x1,y1,x2,y2=line_point[i]
            distance=getdis(k.pt[0],k.pt[1],x1,y1,x2,y2)
            if(distance<md[idx]):
                bi[idx] = i
                md[idx]=distance'''

    #print('pixels',distance,'mm',50/avg*distance,'truth',int(name[:-4]),'dist',abs(50/avg*distance-int(name[:-4])))
    k1 = keypoints[0]
    k2 = keypoints[1]
    bestd = 100000
    bestdd = 0
    for iid,i in enumerate(line_point):
                d1 = line_ab[iid][0]*(k1.pt[0]-line_ab[iid][1])+line_ab[iid][2]-k1.pt[1]
                d2 = line_ab[iid][0]*(k2.pt[0]-line_ab[iid][1])+line_ab[iid][2]-k2.pt[1]
                if(d1*d2<0):
                    #print(line_ab[iid], d1,d2)
                    x1,y1,x2,y2=i
                    #cv.line(img, (x1, y1), (x2, y2), (255, 0, 0), 10)
                    #print(line_point)
    cv.imwrite('picout/'+name[:-4]+'out.bmp',img)




    for iid,i in enumerate(line_point):
        for jjd,j in enumerate(line_point):
            point_x,point_y,px2,py2=i
            x1,y1,x2,y2=j
            dist=getdis(point_x,point_y,x1,y1,x2,y2)
            if(50/avg*dist>100 and abs(line_ab[iid][3]-line_ab[jjd][3])<0.2):
                d1 = line_ab[iid][0]*(k1.pt[0]-line_ab[iid][1])+line_ab[iid][2]-k1.pt[1]
                d2 = line_ab[iid][0]*(k2.pt[0]-line_ab[iid][1])+line_ab[iid][2]-k2.pt[1]
                #print(d1*d2)
                if(d1*d2<0):
                    d1 = line_ab[jjd][0]*(k1.pt[0]-line_ab[jjd][1])+line_ab[jjd][2]-k1.pt[1]
                    d2 = line_ab[jjd][0]*(k2.pt[0]-line_ab[jjd][1])+line_ab[jjd][2]-k2.pt[1]
                    #print(d1*d2)
                    if(d1*d2<0):
                    #if(abs(math.atan(y1/x1)-math.atan(y2/x2))<0.2):
                        #distance= min( getdis(k1.pt[0],k1.pt[1],x1,y1,x2,y2),getdis(k1.pt[0],k1.pt[1],x1,y1,x2,y2))
                        #x1,y1,x2,y2=i
                        #distance= min(distance, getdis(k1.pt[0],k1.pt[1],x1,y1,x2,y2),getdis(k1.pt[0],k1.pt[1],x1,y1,x2,y2))
                        if(dist<bestd):
                            bestd = dist
                            bestdd = dist
    print(bestdd,50/avg*bestdd,int(name[:-4]),abs(50/avg*bestdd-int(name[:-4])))
    for i in line_point:
        for j in line_point:
            point_x,point_y,_,_=i
            x1,y1,x2,y2=j
            dist=getdis(point_x,point_y,x1,y1,x2,y2)
            if (dist==bestdd):
                x1,y1,x2,y2=i
                cv.line(img, (x1, y1), (x2, y2), (255, 0, 0), 10)
                x1,y1,x2,y2=j
                cv.line(img, (x1, y1), (x2, y2), (255, 0, 0), 10)
                cv.imwrite('picout/'+name[:-4]+'out.bmp',img)
    #print('pixels',bestdd,'mm',50/avg*bestdd,'truth',int(name[:-4]),'dist',abs(50/avg*bestdd-int(name[:-4])))


